//
//  Initializer.c
//  markerARLib
//
//  Created by fg on 6/18/16.
//  Copyright © 2016 CvidKal. All rights reserved.
//

#include "Initializer.h"
#include "global.h"

using namespace cv;
using namespace google;


namespace markerAR {
    
    bool Initializer::checkValidMirror(const Eigen::Matrix3d &Hqt,const cv::Size& imgSize)const
    {
        float x[4] = {0,static_cast<float>(imgSize.width),static_cast<float>(imgSize.width),0},y[4] = {0,0,static_cast<float>(imgSize.height),static_cast<float>(imgSize.height)};
        float x2,y2;
        for (int i=0; i<4; ++i) {
            applyHomography(Hqt, x[i], y[i], x2, y2);
            if (x2<0||x2>=imgSize.width||y2<0||y2>=imgSize.height) {
                return false;
            }
        }
        return true;
        
    }
    
    bool Initializer::tryInitialize(const cv::Mat &img,double timeStamp)
    {
        m.lock();
        Mat gray;
        if (img.channels() != 1)
        {
            cvtColor(img, gray, CV_BGR2GRAY);
        }
        else
        {
            gray = img.clone();
        }
        std::vector<cv::KeyPoint> kpts;
        cv::Mat desp;
        orb->operator()(img, Mat(), kpts,desp);
        auto result = bow.query(kpts, desp);
#define NDEBUG
#ifndef NDEBUG
        Mat debug;
        drawKeypoints(img, kpts, debug);
        imshow("kpts", debug);
        cvWaitKey();
#endif
        if (result.size()==0) {
            LOG(WARNING)<<"Initializer: No marker detected!"<<endl;
            m.unlock();
            return false;
        }
        
        auto nowMarker = map->getMarker(result[0].id);
        Mat warp;
        warpPerspective(gray, warp, result[0].Htq, gray.size());
        
        
        orb->operator()(warp, Mat(), kpts, desp);
#ifndef NDEBUG
        drawKeypoints(warp, kpts, debug);
        imshow("warp", debug);
        imshow("marker", nowMarker->img);
        cvWaitKey();
#endif
        std::vector<vector<cv::DMatch>> dmatches;
        binaryMatcher->knnMatch(desp, nowMarker->orbStructure->desp, dmatches,2);
        std::vector<int> idx;
        GetMatch::GetInliersRadius(kpts, nowMarker->orbStructure->kpts, dmatches, idx, 0.6, 90, 10*10);
        if (idx.size()<20) {
            LOG(WARNING)<<"Initializer: No enough Correspondences"<<endl;
            m.unlock();
            return false;
        }
        if (!checkValidMirror(Converter::CV2Eigen(result[0].Htq).inverse(), gray.size())) {
            LOG(WARNING)<<"Initializer: Not a valid Mirror"<<endl;
            m.unlock();
            return false;
        }
        
        
        std::vector<cv::DMatch> dmatch;
        GetMatch::GetDMatch(dmatches, idx, dmatch);
        
        vector<cv::Point2f> now2d2;
        vector<Point3f> marker3D;
        GetMatch::prepare3D_2D(nowMarker->orbStructure->p3d, kpts, dmatch, marker3D, now2d2);
        transformPointsInverse(now2d2, result[0].Htq);
        cv::Matx33d R;
        cv::Matx31d t;
        Solve3D::getCameraRT(K->K_, marker3D, now2d2, R, t, false);
        
        Pose C;
        C.R = Converter::CV2Eigen(R);
        C.t = Converter::CV2Eigen(t);
        cout<<C.t<<endl;
        
        Eigen::Matrix3d MatrixR;
        double yaw = 0,pitch = 0,roll = 0;
        MatrixR =  Solve3D::setRotationMatrix(yaw, pitch, roll);
        nowMarker->markerNow = new Marker(warp,MatrixR,nowMarker->id());
        
        *map->getMarkerPointer(result[0].id) = nowMarker->markerNow;
        nowMarker = map->getMarker(result[0].id);
        
        track->init(C,nowMarker,nowMarker->keyframe.get(),Converter::CV2Eigen(result[0].Htq).inverse(),timeStamp);
        
        return true;
        
        
    }
    
    
    
    
}